%% 混合A*算法搜索路径
% 附加功能要求
% Statistics and Machine Learning Toolbox
% Aerospace Toolbox
% Navigation Toolbox
% Mapping Toolbox
% 输入参数
% ObstTolerance: 障碍物容忍度
% GlobalMap: 全局地图
% Start: 小车起始位置
% End: 小车目标位置
% CarWidth: 小车宽度
% CarLength: 小车长度
% SatLevel: 小车速度与转角改变的限制范围
% Animation: 是否显示动画
% MaxTime: 最大搜索时间，单位为秒
% 输出参数
% x: 小车路径x值
% y: 小车路径y值
% theta: 小车路径朝向
% D: 小车路径距离
% delta: 小车路径转角
% totalDistance: 小车路径总距离
% steeringTimes: 小车路径转向倍数，与速度成反比
function [x,y,theta,D,delta,totalDistance,steeringTimes] = SearchPath(ObstTolerance,GlobalMap,Start,End,CarWidth,CarLength,SatLevel,Animation,MaxTime)
    disp('Start searching path...')
    disp(['   Obstacle tolerance: ',num2str(ObstTolerance)])
    disp(['   Start: (',num2str(Start(1)),',',num2str(Start(2)),',',num2str(Start(3)),')'])
    disp(['   End: (',num2str(End(1)),',',num2str(End(2)),')'])
    disp(['   Car width: ',num2str(CarWidth)])
    disp(['   Car length: ',num2str(CarLength)])
    disp(['   Sat level: (',num2str(SatLevel(1)), ',', num2str(SatLevel(2)),')'])
    disp(['   Animation: ',num2str(Animation)])
    disp(['   Max time: ',num2str(MaxTime)])
    
    % 初始化地图参数
    Configure.XLENGTH = length(GlobalMap(:,1));
    Configure.YLENGTH = length(GlobalMap(1,:));

    % 初始化障碍列表
    Obstacle = [];
    ObstList = [];
    for i = 1:Configure.XLENGTH
        for j = 1:Configure.YLENGTH
            if GlobalMap(i,j) == 1
                % 获取障碍物点
                Obstacle(end+1,:) = [i,j]; %#ok<*AGROW>
                % 获取障碍物列表
                if ~IsMember([i,j],ObstList)
                    ObstList(end+1,:) = [i,j];
                end
                if ~IsMember([i-1,j],ObstList)
                    ObstList(end+1,:) = [i-1,j];
                end
                if ~IsMember([i,j-1],ObstList)
                    ObstList(end+1,:) = [i,j-1];
                end
                if ~IsMember([i-1,j-1],ObstList)
                    ObstList(end+1,:) = [i-1,j-1];
                end
            end
        end
    end
    Configure.OBSTACLE = Obstacle;                  % 障碍物点
    Configure.OBSTACLE_LIST = ObstList;             % 障碍物列表

    % 初始化地图范围
    Configure.MINX = 0;
    Configure.MAXX = Configure.XLENGTH;
    Configure.MINY = 0;
    Configure.MAXY = Configure.YLENGTH;

    % 获取车辆参数
    Vehicle.WB = CarLength;                         % 车辆轴距（长度即为轴距）
    Vehicle.W = CarWidth;                           % 车辆宽度
    Vehicle.LF = CarLength/2;                       % 车辆前部到车辆前端的距离
    Vehicle.LB = CarLength/2;                       % 车辆后部到车辆后端的距离

    % 规定转向角范围
    Configure.MINYAW = -pi;
    Configure.MAXYAW = pi;

    % 设置障碍物参数（自适应）
    Configure.OBSTACLE_TOLERANCE = ObstTolerance;   % 障碍物碰撞容忍度

    % 设置分辨率参数
    Configure.XY_GRID_RESOLUTION = 2.0;             % A*代价图分辨率
    Configure.N_STEER = 20.0;                       % 转向角取样数量
    Configure.YAW_GRID_RESOLUTION = deg2rad(15.0);  % 转向角分辨率

    % 设置多角度搜索参数（自适应）
    Configure.N_STEER_SEARCH = 20;                  % 车辆多角度搜索转角度取样数量
    Configure.N_ANGLE_SEARCH = 4;                   % 车辆多长度搜索角度取样数量
    Configure.MAX_LENGTH_SEARCH = 0.0;              % 车辆多长度搜索最大长度
    Configure.N_LENGTH_SEARCH = 1;                  % 车辆多长度搜索长度取样数量

    % 设置转向角搜索倍数范围（自适应）
    Configure.STEERING_TIMES = 1.0;                 % 转向角相对倍数，2倍代表速度变成1/2倍刚好可以转向
    Configure.STEERING_TIMES_RANGE = ...
        linspace(1.0,6.0,11);                       % 转向角搜索范围

    % 设置时间分辨率搜索范围（自适应）
    Configure.TIME_RESOLUTION = 0.5;                % 时间分辨率，每次取样之间的时间间隔
    Configure.TIME_RESOLUTION_RANGE = ...
        linspace(0.2,0.8,7);                        % 时间分辨率搜索范围

    % 设置代价权重
    Configure.BACK_COST = 1.5;                      % 后退惩罚代价
    Configure.STEER_COST = 1.5;                     % 转向代价
    Configure.STEER_CHANGE_COST = 1.5;              % 转向角变化代价
    Configure.DISTANCE_CHANGE_COST = 0;             % 距离改变代价
    Configure.COSTMAP_COST = 15;                    % 代价图A*算法距离代价

    % 设置路径优化参数
    Configure.END_TOLERANCE = 0.3;                  % 终点容忍度

    % 计算路径分辨率，最大转向角与车辆约束
    Configure.MOTION_RESOLUTION = ...
        Configure.TIME_RESOLUTION*SatLevel(1);      % 路径分辨率
    Vehicle.MAX_STEER = Configure.TIME_RESOLUTION* ...
        Configure.STEERING_TIMES*SatLevel(2);       % 车辆最大转向角
    Vehicle.MIN_CIRCLE = ...
        Vehicle.WB/tan(Vehicle.MAX_STEER);          % 最小转弯半径

    % 寻找相对最佳转向角
    x = [];
    y = [];
    theta = [];
    D = [];
    delta = [];
    
    % 搜索可行的结束位置和转向角
    [bestEnd,bestEnd1,maximalObstDistance] = SearchSteer(End,Vehicle,Configure);

    % 若仍然未找到可行的结束位置和转向角，则增大搜索最大长度
    while maximalObstDistance == -inf
        Configure.MAX_LENGTH_SEARCH = Configure.MAX_LENGTH_SEARCH + 0.5;
        [bestEnd,bestEnd1,maximalObstDistance] = SearchSteer(End,Vehicle,Configure);
    end
    disp(['Find best end: (',num2str(bestEnd(1)),',',num2str(bestEnd(2)),',',num2str(bestEnd(3)),')', ...
        ' with maximal obstacle distance ', num2str(maximalObstDistance), '.']);

    % 计算代价地图
    CostMap = GridAStar(Configure,Vehicle,End,Animation);
    Configure.CostMap = CostMap;
    
    % 最优化搜索路径距离
    tic;  % 记时
    elapsed_time = 0;
    Configure.Start = Start;
    Configure.End = bestEnd;
    minimalPathDistance = inf;
    for i = 1:length(Configure.STEERING_TIMES_RANGE)
        % 取消注释此行以显示地图，起始点与结束点
        % continue;

        % 获取转向角相对倍数
        steeringTimesSample = Configure.STEERING_TIMES_RANGE(i);
        disp(['Start searching ',num2str(steeringTimesSample),' steering time...']);
        Configure.STEERING_TIMES = steeringTimesSample;
        
        % 搜索所有时间分辨率
        for k = 1:length(Configure.TIME_RESOLUTION_RANGE)
            % 获取时间分辨率
            timeResolutionSample = Configure.TIME_RESOLUTION_RANGE(k);
            Configure.TIME_RESOLUTION = timeResolutionSample;
            disp(['   Start searching ',num2str(timeResolutionSample),' time resolution...'])

            % 更新路径分辨率，最大转向角与最小转弯半径
            Configure.MOTION_RESOLUTION = ...
                Configure.TIME_RESOLUTION*SatLevel(1);
            Vehicle.MAX_STEER = Configure.TIME_RESOLUTION* ...
                Configure.STEERING_TIMES*SatLevel(2);
            Vehicle.MIN_CIRCLE = ...
                Vehicle.WB/tan(Vehicle.MAX_STEER);

            % 搜索两个方向的结束位置
            for j = 1:2
                if j == 1
                    Configure.Start = Start;
                    Configure.End = bestEnd;
                    disp('      Start searching endYaw...')
                else
                    Configure.Start = Start;
                    Configure.End = bestEnd1;
                    disp('      Start searching endYaw1...')
                end
                [x1,y1,theta1,D1,delta1,totalDistance1] = SearchPath1(Start,End,Vehicle,Configure);
                if ~isempty(x1)
                    if totalDistance1 < minimalPathDistance
                        x = x1;
                        y = y1;
                        theta = theta1;
                        D = D1;
                        delta = delta1;
                        minimalPathDistance = totalDistance1;
                        steeringTimes = steeringTimesSample;
                        disp(['         Find a better path with distance ',num2str(minimalPathDistance),'.'])
                    else
                        disp(['         Find a path with distance ',num2str(totalDistance1),'.'])
                    end
                end

                % 输出时间间隔并记录时间
                now_elapsed_time = toc;
                interval = now_elapsed_time - elapsed_time;
                disp(['         Elapsed time: ',num2str(interval),'s.'])
                elapsed_time = toc;
                clear now_elapsed_time interval;

                % 时间超过最大时间且寻找到路径则退出
                if elapsed_time > MaxTime && minimalPathDistance ~= inf
                    break;
                end
            end

            % 时间超过最大时间且寻找到路径则退出
            if elapsed_time > MaxTime && minimalPathDistance ~= inf
                break;
            end
        end

        % 时间超过最大时间或寻找到路径则退出（再加大搜索倍数会降低汽车速度）
        if minimalPathDistance ~= inf || elapsed_time > MaxTime
            break;
        end
    end

    % 可视化轨迹
    totalDistance = minimalPathDistance;
    disp(['Total path distance: ',num2str(totalDistance),'.'])
    if ~isempty(x) && ~Animation
        % 以蓝色曲线绘制规划出来的轨迹
        plot(x,y,'b')
    else
        VehicleAnimation(x,y,theta,Configure,Vehicle,Animation);
    end
end

% 判断点是否在障碍物列表中
function [lia,locb] = IsMember(Point,ObstList)
    lia = false;
    locb = -1;
    if isempty(ObstList)
        return;
    end
    for i = 1:length(ObstList(:,1))
        if Point(1) == ObstList(i,1) && Point(2) == ObstList(i,2)
            lia = true;
            locb = i;
            return;
        end
    end
end

% 混合A*算法搜索路径
function [x,y,theta,D,delta,totalDistance] = SearchPath1(Start,End,Vehicle,Configure)
    % 使用完整约束有障碍情况下用A*搜索的最短路径最为hybrid A*的启发值
    [x,y,theta,D,delta] = HybridAStar(Vehicle,Configure);
    if isempty(x)
        x = [];
        y = [];
        theta = [];
        D = [];
        delta = [];
        totalDistance = inf;
        return;
    end

    % 如果找到的路径不是从起点开始的路径，则继续搜索
    flag = 0;
    while flag == 0
        if x(1) ~= Configure.Start(1) || y(1) ~= Configure.Start(2) || theta(1) ~= Configure.Start(3)
            Configure.Start = Start;
            Configure.End = [x(1), y(1), theta(1)];
            [x1,y1,theta1,D1,delta1] = HybridAStar(Vehicle,Configure);
            if isempty(x1)
                x = [];
                y = [];
                theta = [];
                D = [];
                delta = [];
                totalDistance = inf;
                return;
            end
            x = [x1(1:end-1),x]; %#ok<*AGROW>
            y = [y1(1:end-1),y];
            theta = [theta1(1:end-1),theta];
            D = [D1(1:end-1),D];
            delta = [delta1(1:end-1),delta];
            clear x1 y1 theta1 D1 delta1;
        else
            flag = 1;
        end
    end

    % 删除无用数据
    D = D(2:end);
    delta = delta(2:end);

    % 优化路径
    [x,y,theta,D,delta] = OptimizePath(x,y,theta,D,delta,End(1),End(2),Configure);

    % 对整条路径进行碰撞检查
    for i = 1:length(x)
        isCollision = VehicleCollisionCheck([x(i),y(i),theta(i)],Configure,Vehicle);
        if isCollision
            x = [];
            y = [];
            theta = [];
            D = [];
            delta = [];
            totalDistance = inf;
            return;
        end
    end

    % 对整条路径进行越界检查
    for i = 1:length(x)
        outRange = VehicleRangeCheck([x(i),y(i)],Configure,Vehicle);
        if outRange
            x = [];
            y = [];
            theta = [];
            D = [];
            delta = [];
            totalDistance = inf;
            return;
        end
    end

    % 计算总距离
    totalDistance = sum(abs(D));
end